
import socket
import json
import copy
import time

import yaml
import os
import traceback
import sys
import numpy as np

import cv2
import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import Bool,String
from acupoint_msgs.msg import AcupointImagePixel,LegCoordinate

from cv_bridge import CvBridge, CvBridgeError



class AcupointDisplay:
    def __init__(self):
        self.deg2rad=3.1415926/180
        self.origin_image=None
        self.bridge=CvBridge()
        self.acupoint_sub = rospy.Subscriber("/acupoint/acu_filter_image_pixel", AcupointImagePixel, self.acupointCallback) 
        self.img_sub = rospy.Subscriber("/k4a/color/color", Image,self.imgCallback,queue_size=1)
        self.acupoint_display_pub=rospy.Subscriber("/acupoint_name",String,self.acupointNameCallback)
        # self.left_leg_sub = rospy.Subscriber("/acupoint/left_leg_line", LegCoordinate, self.leftLegCallback) 
        self.x_lists=[]
        self.y_lists=[]
        self.jaka_arrive=False
        self.acupoint_name=None
        self.acupoint_pixels=None
        self.display_acupoint=None
        # self.left_coordinate=[-1.0,-1.0,-1.0]
        print("wait for image and acupoint")
        while (not rospy.is_shutdown()) and (self.origin_image is None or self.acupoint_name is None):
            rospy.sleep(0.1)
        
        print("display acupoint")
        flag=0
        while not rospy.is_shutdown():
            for name,pixel in zip(self.acupoint_name,self.acupoint_pixels):
                y_plot = int(pixel.x)
                x_plot = int(pixel.y)
                # print(y_plot)
        
                cv2.circle(self.origin_image, (x_plot, y_plot), 4, (255, 0, 0), thickness=-1)
                # cv2.putText(self.origin_image, name, (x_plot, y_plot), cv2.FONT_HERSHEY_PLAIN,
                #         0.5, (106, 68, 20), thickness=1)
            if self.display_acupoint=="end":
                self.display_acupoint=None
            elif self.display_acupoint=="left_leg":
                index=self.acupoint_name.index("l_BL40")
                x1=self.acupoint_pixels[index].x
                y1=self.acupoint_pixels[index].y
                index=self.acupoint_name.index("l_BL59")
                x2=self.acupoint_pixels[index].x
                y2=self.acupoint_pixels[index].y
                y_plot = int((x1+x2)/2.0)
                x_plot = int((y1+y2)/2.0)
                # print(x_plot)
                # print(y_plot)
                cv2.circle(self.origin_image, (x_plot, y_plot), 8, (0, 255, 255), thickness=-1)   
                
            elif self.display_acupoint is not None:
                
                index=self.acupoint_name.index(self.display_acupoint)
                x=self.acupoint_pixels[index].x
                y=self.acupoint_pixels[index].y
                y_plot = int(x)
                x_plot = int(y)
                cv2.circle(self.origin_image, (x_plot, y_plot), 8, (0, 255, 255), thickness=-1)   
            else:
                pass
            # flag=(flag+1)%10
            # if flag==9:
            #     cv2.imwrite("/home/m/display/"+str(time.time())+".png",self.origin_image)
            cv2.namedWindow("acupoint_display", 0)
            cv2.imshow("acupoint_display", self.origin_image)
            cv2.waitKey(100)
            rospy.sleep(0.1)
            
    # def leftLegCallback(self,msg):
    #     self.left_coordinate[0]=(msg.position[0].x+msg.position[1].x)/2
    #     self.left_coordinate[1]=(msg.position[0].y+msg.position[1].y)/2
    #     self.left_coordinate[2]=(msg.position[0].z+msg.position[1].z)/2
        
        
    def acupointNameCallback(self,msg):
        self.display_acupoint=msg.data   
        print(self.display_acupoint)
         
    def imgCallback(self,image_msg):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
            self.origin_image = cv_image.copy()
            
        except:
            pass
            # print(e)
    
  
            
      
    def acupointCallback(self,msg):
        self.acupoint_name=copy.deepcopy(msg).name
        self.acupoint_pixels=copy.deepcopy(msg).pixels
     
    

        
    # def loadYaml(self,file_path):
    #     if not os.path.exists(file_path):
    #         print("Error, %s not exists"%file_path)
    #         return
    #     yaml_data=None
    #     try:
    #         with open(file_path) as f:
    #             yaml_data=yaml.safe_load(f.read())
    #     except:
    #         print("Some error occured,the exception message is:")
    #         print(traceback.format_exc())
    #     return yaml_data


if __name__ == "__main__":
    rospy.init_node('acupoint_display_node')
    cd=AcupointDisplay()



    

    
